#include "SimTKmolmodel.h"
#include "Molmodel.h"
#include "SimTKsimbody_aux.h"
#include <iostream>
#include <fstream>
#include <exception>
#include "DcaSimtk.h"

using namespace std;
using namespace SimTK;

// Motion constraints on the first residue
mCon PresMo1(0.0,0.0); // On Qindex 4 of residue 1
mCon PresMo2(0.0,0.0); // On Qindex 5 of residue 1
mCon PresMo3(0.0,0.0); // On Qindex 6 of residue 1

int main()
{
try {
  
	// Initialize simbody objects
   
	CompoundSystem system;
	SimbodyMatterSubsystem matter(system);
	DuMMForceFieldSubsystem forces(system);
	forces.loadAmber99Parameters();
	GeneralForceSubsystem GeneralForces(system);
	//Force::Thermostat Therm(GeneralForces, matter,SimTK_BOLTZMANN_CONSTANT_MD,600,1.0);
	NoseHooverThermostat Therm(GeneralForces,matter,300,0.07,6);
	

	////////////////////////////////////////////////////////////////////////////////
	// Initialize molmodel objects
	// Attempting to set the parameters approximately equal to 
	// those obtained from contacts.dat
	
	// Define RNA sequence and bond mobilities
	RNA rna("AAAAAAAAAAAAAAAAAAAA");
	rna.setCompoundBondMobility(BondMobility::Torsion); // set the mobility of every bond in the compound
	
	
	///*
	for(int i=0;i<20;i++){
		rna.setResidueBondMobility(ResidueInfo::Index(i),BondMobility::Rigid);
	}
	//*/

	//6/23/2012: Changed compound mobility to free for all bonds. 
	
	// These paramters are correspond to defualt MD paramters and values
	// are taken from ParameterReader.h of RNABuilder
	/*forces.setCoulombGlobalScaleFactor(1.0);
	forces.setVdwGlobalScaleFactor(1.0);
	forces.setBondTorsionGlobalScaleFactor(1.0);
	forces.setBondBendGlobalScaleFactor(1.0);
    forces.setBondStretchGlobalScaleFactor(1.0);
	forces.setAmberImproperTorsionGlobalScaleFactor(1.0);
	*/
	////////////////////////////////////////////////////////////////////////
	// KDB
	// Note: I have not found an equivalent function for leontiswestof
	// command in contacts.dat.
	////////////////////////////////////////////////////////////////////////

			
	system.adoptCompound(rna);
	
	
	
	system.modelCompounds();	
			
	// Prepare for molecular dynamics
	system.realizeTopology();
	State& state = system.updDefaultState();

	int s = 1;
	Therm.setNumChains(state,s);
	system.realizeModel(state);

	system.realize(state,Stage::Position);
	LocalEnergyMinimizer::minimizeEnergy(system, state, 15.0);	

	
	//Minimized energy free free system
	/*Real arr[11] = {1, 0 ,0 ,0 ,0 ,0 ,0, -1.27409, -1.27409, -1.27409 ,-1.27409};
	for ( int i = 0 ; i < state.getNQ() ; i++){
		state.updQ()[i] = arr[i];
	}*/

	cout << state.getQ();
	//std::cout << state.getQ() << std::endl; 
	//std::cout << state.getU() << std::endl; 
	
	 system.realize(state,Stage::Dynamics);
	    std ::cout << state.getTime() << std::endl;
	    Real KE = matter.calcKineticEnergy(state);
	    Real PE = system.calcPotentialEnergy(state);
	    //Energy.open ( "Energy.txt", ios::app);
	    cout << state.getTime() << "\t" << KE <<  "\t" << PE << "\t" << KE + PE << std::endl;
	    //Energy.close();
	 //for(int i=0;i<state.getNU();++i)
  //      state.updU()[i]=0.1;
  //  system.realize(state,Stage::Velocity); 
		//
	
	// Start DCA part	
	Real AngleThreshold = 0.0;
	Real ForceThreshold = 10000;
	int WidthofMovingWindow = 1;		
	
	DcaPreprocessor DcaObj(system,matter,state);
	DcaObj.Init();	
	Assembly A(DcaObj, system, matter, state,Therm,ForceThreshold);	
	
	DcaIntegrator Integ(A,WidthofMovingWindow,AngleThreshold, Therm,rna);	
	bool Animate = true;	
	Integ.StepTo(30,0.01,Animate);
			
	return 0;

} 
catch(const std::exception& e) {
    std::cerr << "ERROR: " << e.what() << std::endl;
    return 1;
}
catch(...) {
    std::cerr << "ERROR: An unknown exception was raised" << std::endl;
    return 1;
}

}
